#pragma config(Sensor, in1,    sensorLeft,     sensorLineFollower)
#pragma config(Sensor, in2,    sensorMiddle,   sensorLineFollower)
#pragma config(Sensor, in3,    sensorRight,    sensorLineFollower)
#pragma config(Sensor, in4,    clawAngle,      sensorPotentiometer)
#pragma config(Sensor, in5,    armAngle,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonarInput,     sensorSONAR_inch)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex393, openLoop)
#pragma config(Motor,  port5,           arm,           tmotorVex393, openLoop)
#pragma config(Motor,  port6,           claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393, openLoop, reversed)

int w = 0;
int x = 0;
int y = 0;
int z = 0;
int sensorLeftInt = 0;
int sensorRightInt = 0;
int sensorMiddleInt = 0;

task main()
{

	while(z == 0){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 50;
			motor[rightMotor] = 50;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 65;
			motor[rightMotor] = 25;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 25;
			motor[rightMotor] = 65;
		}

		if(sensorRightInt > 2000 && sensorMiddleInt > 2000){
			wait1Msec(450);
			motor[leftMotor] = motor[rightMotor] = 0;
			z++;
		}
	}

	wait1Msec(500);

	sensorLeftInt = 0;
	sensorRightInt = 0;
	sensorMiddleInt = 0;

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	x = 0;
	y = 0;

	while(/*x > -200 && y < 200 && */z == 1){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];
		x = SensorValue[rightEncoder];
		y = SensorValue[leftEncoder];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = -80;
			motor[rightMotor] = 80;
			wait1Msec(45);
			z++;
		}
	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;

	wait1Msec(500);

	sensorLeftInt = 0;
	sensorRightInt = 0;
	sensorMiddleInt = 0;

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	x = 0;
	y = 0;

		while(z == 2){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 50;
			motor[rightMotor] = 50;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 65;
			motor[rightMotor] = 25;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 25;
			motor[rightMotor] = 65;
		}

		if(sensorRightInt > 2000 && sensorMiddleInt > 2000){
			wait1Msec(450);
			motor[leftMotor] = motor[rightMotor] = 0;
			z++;
		}
	}

	wait1Msec(500);

	sensorLeftInt = 0;
	sensorRightInt = 0;
	sensorMiddleInt = 0;

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	x = 0;
	y = 0;

	while(/*x > -200 && y < 200 && */z == 3){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];
		x = SensorValue[rightEncoder];
		y = SensorValue[leftEncoder];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = -80;
			motor[rightMotor] = 80;
			wait1Msec(45);
			z++;
		}
	}

	motor[leftMotor] = 0;
	motor[rightMotor] = 0;

	wait1Msec(500);

	sensorLeftInt = 0;
	sensorRightInt = 0;
	sensorMiddleInt = 0;

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;
	x = 0;
	y = 0;

	w = SensorValue[armAngle];

	if(SensorValue[sonarInput] > 4 && SensorValue[sonarInput] < 15){
		while(w > 325){
			motor[arm] = -90;
		}
	}
}
